/*
 * Copyright (c) 2006-2024, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-07     RT-Thread    first version
 */

#include <rtthread.h>
#include <inc/imu.h>
#include <inc/uwb.h>
#include <inc/timer.h>
#include <inc/main.h>
#include <inc/pid.h>
#include <inc/motor_ctr.h>
#include <inc/uwb.h>
#include <inc/gpio.h>
#include <math.h>

#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

extern rt_timer_t updataTimer;
uint8_t dirZ_cnt = 0;

int main(void)
{

    createTimerTask();
    rt_timer_stop(updataTimer);

    createUwbDeviceAndTask();
    createImuDeviceAndTask();
    MX_GPIO_Init();

    transmitMotorCtor(200,0);
    jp61Init();

    BEEP_ON; //id 1
    rt_thread_delay(200);
    BEEP_OFF;

    rt_kprintf("initinitinit\r\n");


    while(1)
    {
       switch(car_sta) //正在前往目的地
       {
           case GOING:

               rt_timer_start(updataTimer);
               rt_kprintf("g:%.2f,%.2f,%.2f\n",actualPos.x,actualPos.y,actualPos.z);

           break;


           case STOPPED :///正在停车程序

               rt_kprintf("s:%.2f,%.2f,%.2f\n",actualPos.x,actualPos.y,actualPos.z);
               pidx_en = 0;
               pidy_en = 0;
               pidz_en = 1;

               if(path_num == 'B' || path_num == 'D')
                   desiredPos.z = -90;
               else if(path_num == 'A' || path_num == 'C')
                   desiredPos.z = 90;

               while(dirZ_cnt <= 10)
               {
                   if(fabs(desiredPos.z - actualPos.z) < 1.0f)
                       dirZ_cnt++;

               }

               dirZ_cnt = 0;
               pidz_en = 0;
               car_sta = UDLOAD;

           break;

           case UDLOAD: ///下货

              pidx_en = 0;
              pidy_en = 0;
              pidz_en = 0;


              rt_kprintf("u:%.2f,%.2f,%.2f\n",actualPos.x,actualPos.y,actualPos.z);

              transmitMotorCtor(500,1);
              rt_thread_delay(5000);
              transmitMotorCtor(500,0);


              desiredPos.z = 0;
              pidz_en = 1;


              while(dirZ_cnt <= 10)
              {
                  if(fabs(desiredPos.z - actualPos.z) < 1.0f)
                  {
                      dirZ_cnt++;
                  }
              }
                  dirZ_cnt = 0;
                  pidx_en = 1;
                  pidy_en = 1;
                  path_cnt = 4;

                  car_sta = RETURNING;

           break;



           case RETURNING:///正在返回

               rt_kprintf("r:%.2f,%.2f,%.2f\n",actualPos.x,actualPos.y,actualPos.z);

           break;

           case STOP: ///完全

               BEEP_OFF;
               rt_kprintf("S:%.2f,%.2f,%.2f\n",uwbRawDatax,uwbRawDatay,actualPos.z);
               HAL_TIM_Base_Stop_IT(&htim8);
               carSetTextPwm(0);
               path_cnt = 0;


            break;


        }




        rt_thread_delay(100);
    }
    return RT_EOK;
}



/*
 M1  \  头  /  M2



 M4  /  尾  \  M3
*/

///* 定时器 1 超时函数 */
void timerOutCallBack(void *parameter)
{
        rt_kprintf("periodic timer is timeout:%ld \n",  rt_tick_get());

         leftFrontMotor.cnt = -(short)TIM2->CNT;
         rightFrontMotor.cnt = (short)TIM3->CNT;
         rightBackMotor.cnt  = (short)TIM4->CNT;
         leftBackMotor.cnt  = -(short)TIM5->CNT;

         TIM2->CNT = 0;
         TIM3->CNT = 0;
         TIM4->CNT = 0;
         TIM5->CNT = 0;

         leftFrontMotor.speed  =  leftFrontMotor.cnt   * PERIMETER / RPM_VEL * DT; ////  cm/s
         rightFrontMotor.speed =  rightFrontMotor.cnt  * PERIMETER / RPM_VEL * DT;
         rightBackMotor.speed  =  rightBackMotor.cnt   * PERIMETER / RPM_VEL * DT;
         leftBackMotor.speed   =  leftBackMotor.cnt    * PERIMETER / RPM_VEL * DT;


        if(uwbDataFilter(DT) == 1) //无数据更新
        {
            carSetTextPwm(0);
            car_sta = STOP;
        }
        else
        {
            posContorUpdata();
            carSetPwm();

        }

}

